/**
 * @file Frame.cc
 *
 * @brief Visual representation of a 3D cartesian frame
 *
 * @author Tomasz Rudny (rudny@gmail.com)
 * @version 1.0
 */


#include "Frame.h"

Frame::Frame() {
	length = 1.0f;
	colourSchema[0][0][0] = 1.0f; 	colourSchema[0][0][1] = 0.0f; 	colourSchema[0][0][2] = 0.0f;
	colourSchema[0][1][0] = 0.0f; 	colourSchema[0][1][1] = 1.0f; 	colourSchema[0][1][2] = 0.0f;
	colourSchema[0][2][0] = 0.0f; 	colourSchema[0][2][1] = 0.0f; 	colourSchema[0][2][2] = 1.0f;

	colourSchema[1][0][0] = 0.5f; 	colourSchema[1][0][1] = 0.5f; 	colourSchema[1][0][2] = 0.0f;
	colourSchema[1][1][0] = 1.0f; 	colourSchema[1][1][1] = 0.5f; 	colourSchema[1][1][2] = 0.5f;
	colourSchema[1][2][0] = 0.5f; 	colourSchema[1][2][1] = 0.0f; 	colourSchema[1][2][2] = 0.5f;
}

/**
 * Draws the 3 connected axis (cartesian frame representation).
 * The frame is oriented and originates at the position given by the
 * matrix parameter m.
 * @param m the matrix of configuration of the frame
 */
void Frame::draw(const GLfloat *m) {
	glPushMatrix();
	glMultMatrixf(m);

	glBegin(GL_LINES);
	{
		glColor3f(1.0f, 0.0f, 0.0f);
		glVertex3f(0.0f, 0.0f, 0.0f);
		glVertex3f(length, 0.0f, 0.0f);

		glColor3f(0.0f, 1.0f, 0.0f);
		glVertex3f(0.0f, 0.0f, 0.0f);
		glVertex3f(0.0f, length, 0.0f);

		glColor3f(0.0f, 0.0f, 1.0f);
		glVertex3f(0.0f, 0.0f, 0.0f);
		glVertex3f(0.0f, 0.0f, length);
	}
	glEnd();

	glPopMatrix();
}

/**
 * Draws the 3 connected axis (cartesian frame representation).
 * The frame is oriented and originates at the position given by the
 * matrix parameter m.
 * @param m Transformation of configuration of the frame
 */
void Frame::draw(const RSIM::Transform &tm, int colourIndex) {
	// convert RSIM::Transform into GLfloat matrix
	const double *r = tm.R().Ptr(), *p = tm.P().Ptr();
	GLfloat m[] = { (GLfloat)r[0], (GLfloat)r[3], (GLfloat)r[6], 0.0f, (GLfloat)r[1],
			(GLfloat)r[4], (GLfloat)r[7], 0.0f, (GLfloat)r[2], (GLfloat)r[5],
			(GLfloat)r[8], 0.0f, (GLfloat)p[0], (GLfloat)p[1], (GLfloat)p[2], 1.0f };

	glPushMatrix();
	glMultMatrixf(m);

	glPushMatrix();
	GLUquadricObj* qObj = gluNewQuadric();
    glColor3f(colourSchema[colourIndex][0][0], colourSchema[colourIndex][0][1],
    		colourSchema[colourIndex][0][2]);
	glRotatef(90.0f, 0.0f, 1.0f, 0.0f);
	gluCylinder(qObj, 0.05, 0.05, 1.0, 20, 10);
	glTranslatef(0.0f, 0.0f, 1.0f);
	gluCylinder(qObj, 0.1, 0.0, 0.2, 20, 10);
	glPopMatrix();
	glPushMatrix();
    glColor3f(colourSchema[colourIndex][1][0], colourSchema[colourIndex][1][1],
    		colourSchema[colourIndex][1][2]);
	glRotatef(-90.0f, 1.0f, 0.0f, 0.0f);
	gluCylinder(qObj, 0.05, 0.05, 1.0, 20, 10);
	glTranslatef(0.0f, 0.0f, 1.0f);
	gluCylinder(qObj, 0.1, 0.0, 0.2, 20, 10);
	glPopMatrix();
	glPushMatrix();
    glColor3f(colourSchema[colourIndex][2][0], colourSchema[colourIndex][2][1],
    		colourSchema[colourIndex][2][2]);
	gluCylinder(qObj, 0.05, 0.05, 1.0, 20, 10);
	glTranslatef(0.0f, 0.0f, 1.0f);
	gluCylinder(qObj, 0.1, 0.0, 0.2, 20, 10);
	glPopMatrix();

	gluDeleteQuadric(qObj);
/*	glBegin(GL_LINES);
	{
		glColor3f(1.0f, 0.0f, 0.0f);
		glVertex3f(0.0f, 0.0f, 0.0f);
		glVertex3f(length, 0.0f, 0.0f);

		glColor3f(0.0f, 1.0f, 0.0f);
		glVertex3f(0.0f, 0.0f, 0.0f);
		glVertex3f(0.0f, length, 0.0f);

		glColor3f(0.0f, 0.0f, 1.0f);
		glVertex3f(0.0f, 0.0f, 0.0f);
		glVertex3f(0.0f, 0.0f, length);
	}
	glEnd();
*/
	glPopMatrix();
}

/**
 * Draws the 3 connected axis (cartesian frame representation).
 * The frame is oriented and originates at the position given by the
 * matrix parameter m.
 * @param m Transformation of configuration of the frame
 */
void Frame::drawBase(const RSIM::Transform &tm, int colourIndex) {
	// convert RSIM::Transform into GLfloat matrix
	const double *r = tm.R().Ptr(), *p = tm.P().Ptr();
	GLfloat m[] = { (GLfloat)r[0], (GLfloat)r[3], (GLfloat)r[6], 0.0f,
			(GLfloat)r[1], (GLfloat)r[4], (GLfloat)r[7], 0.0f, (GLfloat)r[2],
			(GLfloat)r[5], (GLfloat)r[8], 0.0f, (GLfloat)p[0], (GLfloat)p[1],
			(GLfloat)p[2], 1.0f };

	glPushMatrix();
	glMultMatrixf(m);

	glPushMatrix();
	GLUquadricObj* qObj = gluNewQuadric();
    glColor3f(colourSchema[colourIndex][0][0], colourSchema[colourIndex][0][1],
    		colourSchema[colourIndex][0][2]);
	glRotatef(90.0f, 0.0f, 1.0f, 0.0f);
	gluCylinder(qObj, 0.025, 0.025, 5.0, 20, 10);
	glTranslatef(0.0f, 0.0f, 5.0f);
	gluCylinder(qObj, 0.06, 0.0, 0.2, 20, 10);
	glPopMatrix();
	glPushMatrix();
    glColor3f(colourSchema[colourIndex][1][0], colourSchema[colourIndex][1][1],
    		colourSchema[colourIndex][1][2]);
	glRotatef(-90.0f, 1.0f, 0.0f, 0.0f);
	gluCylinder(qObj, 0.025, 0.025, 5.0, 20, 10);
	glTranslatef(0.0f, 0.0f, 5.0f);
	gluCylinder(qObj, 0.06, 0.0, 0.2, 20, 10);
	glPopMatrix();
	glPushMatrix();
    glColor3f(colourSchema[colourIndex][2][0], colourSchema[colourIndex][2][1],
    		colourSchema[colourIndex][2][2]);
	gluCylinder(qObj, 0.025, 0.025, 5.0, 20, 10);
	glTranslatef(0.0f, 0.0f, 5.0f);
	gluCylinder(qObj, 0.06, 0.0, 0.2, 20, 10);
	glPopMatrix();

	gluDeleteQuadric(qObj);
/*	glBegin(GL_LINES);
	{
		glColor3f(1.0f, 0.0f, 0.0f);
		glVertex3f(0.0f, 0.0f, 0.0f);
		glVertex3f(length, 0.0f, 0.0f);

		glColor3f(0.0f, 1.0f, 0.0f);
		glVertex3f(0.0f, 0.0f, 0.0f);
		glVertex3f(0.0f, length, 0.0f);

		glColor3f(0.0f, 0.0f, 1.0f);
		glVertex3f(0.0f, 0.0f, 0.0f);
		glVertex3f(0.0f, 0.0f, length);
	}
	glEnd();
*/
	glPopMatrix();
}
